/*************************************************************************
  Crytek Source File.
  Copyright (C), Crytek Studios, 2001-2010.
 -------------------------------------------------------------------------
  $Id$
  $DateTime$
  
 -------------------------------------------------------------------------
  History:
  - Oct 5th, 2010  : Created by Michelle Martin
 *************************************************************************/

#include "StdAfx.h"
#include <IEntitySystem.h>		// For IEntity and GetEntity(id)
#include <IRenderAuxGeom.h>		// For DrawSphere
#include <ICryAnimation.h>		// ForCharacterInstance interfaces

#include "IKTargetBase.h"

const float LIMB_ANIM_BLENDIN_THRESHOLD = 0.7f; // 70% of the way


CIKTarget_Base::CIKTarget_Base(const SIKTargetParams& ikParams, QuatT &startPosition)
: m_currentWorldPos( IDENTITY )
{
	m_ikParameters = ikParams;
	m_blendHelperPos = startPosition;

	// if no blend-time is desired, then the blending in can be skipped
	if (ikParams.blendInTimeSecs <= 0.0f)
	{
		m_currentIKState = IK_FULLY_BLENDED_IN;
	}
	else
	{
		m_currentIKState = IK_BLENDING_IN;
		m_blendHelperTime = ikParams.blendInTimeSecs;
		
		// Make the start position a local one - IF one was provided
		if (!m_blendHelperPos.IsIdentity())
			m_blendHelperPos = QuatT(gEnv->pEntitySystem->GetEntity(ikParams.characterEntityID)->GetWorldTM().GetInverted()) * m_blendHelperPos;

	}
}

// ==========================================================

void CIKTarget_Base::Update( float frameTime, int offset )
{
	m_targetWorldPos = UpdateWorldPos(frameTime);
	m_currentWorldPos = m_targetWorldPos;

	switch (m_currentIKState)
	{
	case IK_BLENDING_IN:
		{
			UpdateBlendingIn(frameTime, offset);
		}
		break;
	case IK_FULLY_BLENDED_IN:
		{
			// Apply Local Offset
			m_currentWorldPos = ApplyOffset(m_currentWorldPos, 1.0f);

			// Debug Output
			if (offset >= 0)
			{
				float colorWhite[4] = { 1,1,1,1 };
				float debugWidth = 350.0f;
				gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 10, 3, colorWhite, false, m_ikParameters.ikHandleName.c_str());
				gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 40, 2, colorWhite, false, "STATUS: Active, 100%% blended in");
			}
		}
		break;
	case IK_BLENDING_OUT:
		{
			UpdateBlendingOut(frameTime, offset);
		}
		break;
	}
}

//====================================================================

void CIKTarget_Base::UpdateBlendingIn( float frameTime, int offset /*= -1*/ )
{
	// to determine whether or not to start limb animation at the end
	float prevBlendVal = GetCurrentBlendVal();

	// check if this frame the end position will be reached
	if (frameTime >= m_blendHelperTime)
	{
		m_blendHelperTime = 0.0f;
		m_currentIKState = IK_FULLY_BLENDED_IN;

		// Apply local offset
		m_currentWorldPos = ApplyOffset(m_currentWorldPos, 1.0f);
	}
	else
	{
		// update blending time
		m_blendHelperTime -= frameTime;

		// get the position to blend over from
		QuatT startPos;
		if (!m_blendHelperPos.IsIdentity())
		{
			// Calculate the start position from local to world
			startPos = QuatT(gEnv->pEntitySystem->GetEntity(m_ikParameters.characterEntityID)->GetWorldTM()) * m_blendHelperPos;
		}
		else
			startPos = GetLimbEndeffectorWorldPos(m_ikParameters.characterEntityID, m_ikParameters.ikHandleHash);

		float timeVal = m_blendHelperTime / m_ikParameters.blendInTimeSecs;
		QuatT tempBlendedWorldPos;

		// Blend from the start position to the world position
		tempBlendedWorldPos.t = m_currentWorldPos.t + (startPos.t - m_currentWorldPos.t) * timeVal;

		// Blend/Interpolate the orientation
		tempBlendedWorldPos.q.SetNlerp(m_currentWorldPos.q, startPos.q, timeVal);

		// Add the local Offset
		tempBlendedWorldPos = ApplyOffset(tempBlendedWorldPos, timeVal, startPos.q);

		// Debug Output
		if (offset >= 0)
		{
			float colorWhite[4] = { 1,1,1,1 };
			float colorBlue[4] = { 0.2f,0.2f,1,1 };
			float colorPurple[4] = { 1,0,1,1 };
			float colorYellow[4] = { 1,1,0,1 };
			float debugWidth = 350.0f;
			// Output state name
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 10, 3, colorWhite, false, m_ikParameters.ikHandleName.c_str());
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 40, 2, colorWhite, false, "STATUS: Blending In...");
			// Output current and overall blendTime
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 60, 2, colorWhite, false, "Blend In Time: %.2f  Current: %.2f", m_ikParameters.blendInTimeSecs, m_ikParameters.blendInTimeSecs-m_blendHelperTime);
			// Output final target world pos (text and sphere) in BLUE
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 80, 2, colorBlue, false, "Target Pos (%.2f | %.2f | %.2f)", m_currentWorldPos.t.x, m_currentWorldPos.t.y, m_currentWorldPos.t.z);
			gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(m_currentWorldPos.t, 0.05f, ColorB(30,30,255));
			// Output hand start position (text and sphere)
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 100, 2, colorPurple, false, "Start Pos (%.2f | %.2f | %.2f)", startPos.t.x, startPos.t.y, startPos.t.z);
			gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(startPos.t, 0.05f, ColorB(255,0,255));
			// Output blended world position (text and sphere)
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 120, 2, colorYellow, false, "Blended Pos (%.2f | %.2f | %.2f)", tempBlendedWorldPos.t.x, tempBlendedWorldPos.t.y, tempBlendedWorldPos.t.z);
			gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(tempBlendedWorldPos.t, 0.06f, ColorB(255,255,0));
		}

		m_currentWorldPos = tempBlendedWorldPos;

	}

	// Check whether it is time to start the animation on the limb
	if (prevBlendVal < LIMB_ANIM_BLENDIN_THRESHOLD && GetCurrentBlendVal() > LIMB_ANIM_BLENDIN_THRESHOLD)
	{
		StartLimbAnimation();
	}
}

//====================================================================

void CIKTarget_Base::UpdateBlendingOut( float frameTime, int offset /*= -1*/ )
{
	if (frameTime >= m_blendHelperTime)
	{
		m_blendHelperTime = 0.0f;
		m_currentWorldPos = GetLimbEndeffectorWorldPos(m_ikParameters.characterEntityID, m_ikParameters.ikHandleHash);
		m_currentIKState = IK_FULLY_FADED_OUT;
	}
	else
	{
		// Calculate the end position (original end-effector pos)
		QuatT endPos = GetLimbEndeffectorWorldPos(m_ikParameters.characterEntityID, m_ikParameters.ikHandleHash);

		// turn the local blending helper into a position in world space 
		QuatT helperPosWorld = QuatT(gEnv->pEntitySystem->GetEntity(m_ikParameters.characterEntityID)->GetWorldTM()) * m_blendHelperPos;

		float timeVal = frameTime / m_blendHelperTime;
		QuatT tempBlendedWorldPos;

		// Blend between current helper pos and end pos
		tempBlendedWorldPos.t = helperPosWorld.t + (endPos.t - helperPosWorld.t) * timeVal;

		// Blend/Interpolate the orientation
		tempBlendedWorldPos.q.SetNlerp(helperPosWorld.q, endPos.q, timeVal);

		// Debug Output
		if (offset >= 0)
		{
			float colorWhite[4] = { 1,1,1,1 };
			float colorBlue[4] = { 0.2f,0.2f,1,1 };
			float colorYellow[4] = { 1,1,0,1 };
			float debugWidth = 350.0f;
			// Output state name
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 10, 3, colorWhite, false, m_ikParameters.ikHandleName.c_str());
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 40, 2, colorWhite, false, "STATUS: Blending Out...");
			// Output current blendTime
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 60, 2, colorWhite, false, "Blend Out: %.2f", m_blendHelperTime);
			// Output final target world pos (text and sphere) in BLUE
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 80, 2, colorBlue, false, "Target Pos (%.2f | %.2f | %.2f)", m_currentWorldPos.t.x, m_currentWorldPos.t.y, m_currentWorldPos.t.z);
			gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(endPos.t, 0.05f, ColorB(30,30,255));
			// Output blended world position (text and sphere)
			gEnv->pRenderer->Draw2dLabel(20 + offset * debugWidth, 120, 2, colorYellow, false, "Blended Pos (%.2f | %.2f | %.2f)", tempBlendedWorldPos.t.x, tempBlendedWorldPos.t.y, tempBlendedWorldPos.t.z);
			gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(tempBlendedWorldPos.t, 0.06f, ColorB(255,255,0));
		} 

		// update blending time
		m_blendHelperTime -= frameTime;

		// update m_blendHelperPos (local)
		m_blendHelperPos = QuatT(gEnv->pEntitySystem->GetEntity(m_ikParameters.characterEntityID)->GetWorldTM().GetInverted()) * tempBlendedWorldPos;

		m_currentWorldPos= tempBlendedWorldPos;
	}

}

//====================================================================

const QuatT& CIKTarget_Base::GetCurrentWorldPos() const
{
	return m_currentWorldPos;
}

//====================================================================

void CIKTarget_Base::FadeOut(float fadeOutTimeInSecs)
{
	m_currentIKState = IK_BLENDING_OUT;

	// Make the current World Pos a local one
	m_blendHelperPos = QuatT(gEnv->pEntitySystem->GetEntity(m_ikParameters.characterEntityID)->GetWorldTM().GetInverted()) * m_currentWorldPos;
	m_blendHelperTime = max(fadeOutTimeInSecs, 0.0f);
	m_ikParameters.blendInTimeSecs = m_blendHelperTime; 

	StopLimbAnimation(fadeOutTimeInSecs * 0.2f); // 20% of the overall blend-out time for the hands
}

//====================================================================

EIK_States CIKTarget_Base::GetIKState() const
{
	return m_currentIKState;
}

//====================================================================

const bool CIKTarget_Base::GetEndEffectorAlignPolicy() const
{
	return m_ikParameters.alignEndEffector;
}

//====================================================================
/*
QuatT CIKTarget_Base::GetLimbEndeffectorWorldPos( EntityId entityID, const char* ikHandleName )
{
	return GetLimbEndeffectorWorldPos(entityID, ikHandleName);
}
*/
QuatT CIKTarget_Base::GetLimbEndeffectorWorldPos( EntityId entityID, uint64 ikHandle )
{
	IEntity* pCharEntity = gEnv->pEntitySystem->GetEntity(entityID);
	ICharacterInstance* pCharacterInstance = pCharEntity->GetCharacter(0);
	int endEffectorIdx = pCharacterInstance->GetICharacterModel()->GetICharacterModelSkeleton()->GetLimbEndeffectorIdx(ikHandle);
	return QuatT(pCharEntity->GetWorldTM()) * pCharacterInstance->GetISkeletonPose()->GetAbsJointByID(endEffectorIdx);
}

//====================================================================

QuatT CIKTarget_Base::GetLimbRootWorldPos( EntityId entityID, uint64 ikHandle )
{
	IEntity* pCharEntity = gEnv->pEntitySystem->GetEntity(entityID);
	ICharacterInstance* pCharacterInstance = pCharEntity->GetCharacter(0);
	int endEffectorIdx = pCharacterInstance->GetICharacterModel()->GetICharacterModelSkeleton()->GetLimbRootIdx(ikHandle);
	return QuatT(pCharEntity->GetWorldTM()) * pCharacterInstance->GetISkeletonPose()->GetAbsJointByID(endEffectorIdx);
}

QuatT CIKTarget_Base::GetCurrentLimbRootWorldPos()
{
	return GetLimbRootWorldPos(m_ikParameters.characterEntityID, m_ikParameters.ikHandleHash);
}

//====================================================================

const uint64 CIKTarget_Base::GetSpinePolicy() const
{
	return m_ikParameters.ikSpineHash;
}

//====================================================================

const float CIKTarget_Base::GetCurrentBlendVal() const
{
	if (m_currentIKState == IK_FULLY_BLENDED_IN)
		return 1.0f;

	if (m_currentIKState == IK_FULLY_FADED_OUT)
		return 0.0f;

	if (m_currentIKState == IK_BLENDING_IN)
		return (m_ikParameters.blendInTimeSecs - m_blendHelperTime) / m_ikParameters.blendInTimeSecs;

	if (m_currentIKState == IK_BLENDING_OUT)
		return (1.0f - (m_ikParameters.blendInTimeSecs - m_blendHelperTime) / m_ikParameters.blendInTimeSecs);

	return 0.0f;
}

const QuatT& CIKTarget_Base::GetCurrentTargetWorlPos() const
{
	return m_targetWorldPos;
}

//====================================================================

QuatT CIKTarget_Base::ApplyOffset( const QuatT& worldLoc, float blendVal, Quat blendRot /*= Quat(IDENTITY)*/ )
{
	QuatT newLoc;
	if (m_ikParameters.offsetIsGlobal == false)
	{
		// Default Case
		if (blendVal >= 1.0f)
		{
			newLoc = (worldLoc * m_ikParameters.localOffset);
		}
		else
		{
			QuatT tempLocalOffset;
			tempLocalOffset.SetNLerp(m_ikParameters.localOffset, QuatT(IDENTITY), blendVal);
			newLoc = (worldLoc * tempLocalOffset);
		}
	}
	else
	{
		newLoc.t = worldLoc.t + m_ikParameters.localOffset.t;
		if (blendVal >= 1.0f)
		{
			newLoc.q = m_ikParameters.localOffset.q;
		}
		else
		{
			newLoc.q.SetNlerp(m_ikParameters.localOffset.q, blendRot, blendVal);
		}
	}

	newLoc.q.NormalizeSafe();
	return newLoc;
}

//====================================================================

void CIKTarget_Base::StartLimbAnimation()
{
	if (m_ikParameters.limbAnimName.empty())
		return;

	// blend in the limb animation on it's layer
	IEntity* pEntity = gEnv->pEntitySystem->GetEntity(m_ikParameters.characterEntityID);
	ICharacterInstance* pCharacterInstance = pEntity->GetCharacter(0);
	if (pCharacterInstance != NULL)
	{
		ISkeletonAnim* pSkeletonAnim = pCharacterInstance->GetISkeletonAnim();
		if (pSkeletonAnim != NULL)
		{
			// prepare animation parameters
			CryCharAnimationParams AParams;
			AParams.m_nLayerID				= m_ikParameters.limbAnimLayer;
			AParams.m_fPlaybackSpeed		= 1.0f;
			AParams.m_fTransTime			= m_blendHelperTime + 0.1f; // a little longer that it will take the IK to blend in
			AParams.m_fAllowMultilayerAnim	= 1;
			AParams.m_nFlags |= CA_ALLOW_ANIM_RESTART | CA_LOOP_ANIMATION; 
			bool bAnimStarted = pSkeletonAnim->StartAnimation(m_ikParameters.limbAnimName.c_str(), AParams);
		}
	}
}

//====================================================================

void CIKTarget_Base::StopLimbAnimation(float blendOutTime)
{
	if (m_ikParameters.limbAnimName.empty())
		return;

	IEntity* pEntity = gEnv->pEntitySystem->GetEntity(m_ikParameters.characterEntityID);
	ICharacterInstance* pCharacterInstance = pEntity->GetCharacter(0);
	if (pCharacterInstance != NULL)
	{
		ISkeletonAnim* pSkeletonAnim = pCharacterInstance->GetISkeletonAnim();
		if (pSkeletonAnim != NULL)
		{
			// prepare animation parameters
			pSkeletonAnim->StopAnimationInLayer(m_ikParameters.limbAnimLayer, blendOutTime);
		}
	}
}

//====================================================================

void CIKTarget_Base::SetOffset( const QuatT& newOffset )
{
	m_ikParameters.localOffset = newOffset;
}

//====================================================================


